<xsl:stylesheet version="2.0"
  xmlns:xsl="http://www.w3.org/1999/XSL/Transform"
  xmlns="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Runtime"
  xmlns:r="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Runtime"
  xmlns:k="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Agents.Core.Kinect"
  xmlns:l="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Navigation.Runtime.Localization"
  xmlns:m="http://schemas.datacontract.org/2004/07/Test.Robotics.Navigation"
  xmlns:n="http://schemas.datacontract.org/2004/07/Microsoft.Robotics.Numerics"
  xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"
  xmlns:i="http://www.w3.org/2001/XMLSchema-instance"
  xmlns:w="http://schemas.datacontract.org/2004/07/Test.Robotics.Navigation.Metrics.PathPlanningAndTracking"
  exclude-result-prefixes="r k l m n a i">

  <xsl:include href="Common.xslt" />

  <xsl:template mode="default" match="text()[.='$map']">\\robofile\baker\Shared\Users\roboim\EnvironmentConfig\CementLoop\BigCementLoop.xml</xsl:template>

  <!-- Starting posing -->
  <xsl:template match="r:Agent[r:Name='StaticMapLocalization' or r:Name='SLAM2DAgent']/l:StartPose">
    <l:StartPose>
      <n:heading>0</n:heading>
      <n:x>200</n:x>
      <n:y>2000</n:y>
    </l:StartPose>
  </xsl:template>

  <!-- Starting pose variance -->
  <xsl:template match="r:Agent[r:Name='StaticMapLocalization']/l:StartPoseVariance">
    <l:StartPoseVariance>
      <n:heading>0.1</n:heading>
      <n:x>5</n:x>
      <n:y>5</n:y>
    </l:StartPoseVariance>
  </xsl:template>

  <!-- Navigation waypoints (big cement loop) -->
  <xsl:variable name="dock">
    <w:Waypoint>
      <w:IsDock>true</w:IsDock>
      <w:IsMarked>false</w:IsMarked>
      <w:Pose>
        <n:heading>0</n:heading>
        <n:x>220</n:x>
        <n:y>2000</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0</n:heading>
        <n:x>0</n:x>
        <n:y>0</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="a">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>0</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>291.92084442061474</n:x>
        <n:y>1997.9327036343614</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0.019191686849272749</n:heading>
        <n:x>0.17918130176396169</n:x>
        <n:y>-0.030044358151072859</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="b">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>1</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>1122.7610575594804</n:x>
        <n:y>1941.9390733994985</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0.15535599236111697</n:heading>
        <n:x>0.010073985292184062</n:x>
        <n:y>-0.023879674172433588</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="c">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>2</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>2075.3248569884795</n:x>
        <n:y>2080.1650167921985</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0.041996486553863122</n:heading>
        <n:x>0.013875688514353612</n:x>
        <n:y>0.029472059882404089</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="d">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>3</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>2879.9547554261076</n:x>
        <n:y>1983.6673721287618</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>-0.131427467523082</n:heading>
        <n:x>0.020984080087664836</n:x>
        <n:y>-0.0036779378234426119</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="e">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>4</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>2800.7423278561737</n:x>
        <n:y>1370.9939487452791</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>-3.0051904925742017</n:heading>
        <n:x>-0.023251239221679043</n:x>
        <n:y>0.0038403343289230563</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="f">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>5</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>1096.4852978879514</n:x>
        <n:y>1565.1670233576626</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0.0932004535081834</n:heading>
        <n:x>-0.010829119044419855</n:x>
        <n:y>-0.016538846228759674</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="g">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>6</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>624.609142516374</n:x>
        <n:y>1296.2748986649199</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0.16081494621671322</n:heading>
        <n:x>0.068523508030931879</n:x>
        <n:y>-0.019589524606359987</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>
  <xsl:variable name="h">
    <w:Waypoint>
      <w:IsDock>false</w:IsDock>
      <w:IsMarked>true</w:IsMarked>
      <w:LandmarkId>7</w:LandmarkId>
      <w:Pose>
        <n:heading>NaN</n:heading>
        <n:x>216.5938659245416</n:x>
        <n:y>1309.2034240420712</n:y>
      </w:Pose>
      <w:RelativeToLandmark>
        <n:heading>0.042423836577642149</n:heading>
        <n:x>-0.0058737641688324238</n:x>
        <n:y>0.017868939529112282</n:y>
      </w:RelativeToLandmark>
    </w:Waypoint>
  </xsl:variable>

  <!-- random traversal of the "cement" course (equivalent to test 1) -->
  <!-- see: \\robofile\baker\Shared\Users\roboim\Nav\Waypoints.txt -->
  <xsl:template name="big-cement-crazy-circuit">
    <xsl:copy-of select="$h" />
    <xsl:copy-of select="$b" />
    <xsl:copy-of select="$c" />
    <xsl:copy-of select="$a" />
    <xsl:copy-of select="$g" />
    <xsl:copy-of select="$d" />
    <xsl:copy-of select="$f" />
    <xsl:copy-of select="$a" />
    <xsl:copy-of select="$f" />
    <xsl:copy-of select="$e" />
    <xsl:copy-of select="$c" />
    <xsl:copy-of select="$d" />
    <xsl:copy-of select="$h" />
    <xsl:copy-of select="$b" />
    <xsl:copy-of select="$a" />
    <xsl:copy-of select="$dock" />
  </xsl:template>

  <!-- loop around the "cement" course -->
  <xsl:template name="big-cement-circuit">
    <xsl:copy-of select="$a" />
    <xsl:copy-of select="$b" />
    <xsl:copy-of select="$c" />
    <xsl:copy-of select="$d" />
    <xsl:copy-of select="$e" />
    <xsl:copy-of select="$f" />
    <xsl:copy-of select="$g" />
    <xsl:copy-of select="$h" />
    <xsl:copy-of select="$dock" />
  </xsl:template>

  <!-- Metrics targets -->
  <xsl:template match="r:Agent[r:Name='PathPlanningAndTrackingMetricsControllerAgent']/m:Waypoints">
    <m:Waypoints>
      <xsl:call-template name="big-cement-circuit" />
    </m:Waypoints>
  </xsl:template>

</xsl:stylesheet>
